#include <ros/ros.h> 
#include <serial/serial.h>  //ROS已经内置了的串口包 
#include <std_msgs/String.h> 
#include <std_msgs/Empty.h> 
#include <std_msgs/Int32MultiArray.h>
#include <geometry_msgs/WrenchStamped.h>
#include <geometry_msgs/TransformStamped.h>
#include <string>
#include <vector>
#include <visualization_msgs/Marker.h>
#include <sstream>
#include <tf/transform_datatypes.h>

using namespace std;

serial::Serial ser; //声明串口对象
geometry_msgs::Point S1_pos;
geometry_msgs::Point L1_pos;
float L_radius = 0.04;
float S_radius = 0.03;

//回调函数 
// void write_callback(const std_msgs::String::ConstPtr& msg) 
// { 
//     ROS_INFO_STREAM("Writing to serial port" <<msg->data); 
//     ser.write(msg->data);   //发送串口数据 
// } 

//实现2个功能：字符串分割为8个力的数组；计算合力
//数据流 force_string(string) to force_data(vector<int>) to wrench_data(WrenchStamped)

// std::vector<int> string_split(std::string force_string){
//     ROS_INFO_STREAM("spliting");
//     std::vector<int> force_data(6,0);
//      for (int i = 0; i < 7; i++){
//         std::string temp = force_string.substr(0,3);
//         cout<<temp;
//         force_data.push_back(stoi(temp));
//         force_string = force_string.substr(4,force_string.size());
//     }
//     force_data.push_back(stoi(force_string));
//     return force_data;
//     ROS_INFO_STREAM("split"<<force_data[0]<<force_data[1]);
// }

// geometry_msgs::WrenchStamped resultant_wrench(std::string force_string){

//     std::vector<int> force_data(6,0);
//     force_data = string_split(force_string);
//     geometry_msgs::WrenchStamped wrench_data;
//     wrench_data.wrench.force.x = ((force_data[0]-force_data[4])+(force_data[3]-force_data[7]))*cos(0.375)+((force_data[1]-force_data[5])+(force_data[2]-force_data[6]))*sin(0.125);
//     wrench_data.wrench.force.y = ((force_data[0]-force_data[4])-(force_data[3]-force_data[7]))*sin(0.375)+((force_data[1]-force_data[5])-(force_data[2]-force_data[6]))*cos(0.125);

//     return wrench_data;
   

// }

geometry_msgs::WrenchStamped wrench;


void wrenchcallback(const geometry_msgs::WrenchStampedConstPtr& msg){
wrench = (pow(msg->wrench.force.x,2)+pow(msg->wrench.force.y,2)+pow(msg->wrench.force.z,2))> (pow(wrench.wrench.force.x,2)+pow(wrench.wrench.force.y,2)+pow(wrench.wrench.force.z,2)) ? *msg :wrench;
}

int main (int argc, char** argv) 
{ 
    //初始化节点 
    ros::init(argc, argv, "tactile_sensor_node"); 
    //声明节点句柄 
    ros::NodeHandle nh; 
    wrench.wrench.force.x = wrench.wrench.force.y = wrench.wrench.force.z = 0.0;
    wrench.wrench.torque.x = wrench.wrench.torque.y = wrench.wrench.torque.z = 0.0;

    //订阅主题，并配置回调函数 
    //ros::Subscriber write_sub = nh.subscribe("write", 1000, write_callback); 
    //发布主题

    ros::Subscriber force0_sub = nh.subscribe("/force_torque_topic0",1000,wrenchcallback);
    ros::Subscriber force1_sub = nh.subscribe("/force_torque_topic1",1000,wrenchcallback);
    ros::Subscriber force2_sub = nh.subscribe("/force_torque_topic2",1000,wrenchcallback);
    ros::Subscriber force3_sub = nh.subscribe("/force_torque_topic3",1000,wrenchcallback);
    ros::Publisher wrench_pub = nh.advertise<geometry_msgs::WrenchStamped>("/force_torque_topic", 1000); 


    //指定循环的频率 

    ros::Rate loop_rate(50); 
    while(ros::ok()) 
    {

                wrench_pub.publish(wrench); 
                ros::spinOnce(); 
                loop_rate.sleep(); 
                
                //read_data_pub.publish(read_data);
            }
        

                   
    return 0;

        //处理ROS的信息，比如订阅消息,并调用回调函数 

    } 
